// Copyright Viet-Dung D. Hoang 2008
// All rights reserved.

// Modified by Minh-Tri Pham 2009

#include <algorithm>
#include <sdcpp/modular_arithmetic.hpp>
#include <sdopencv/polyint.hpp>

using namespace std;
using namespace sdcpp;
using namespace sdopencv;

PolygonalIntegrator::PolygonalIntegrator(int max_width, int max_height, int r2)
{
    //
    img = NULL;
    vertical_sum = NULL;
    integral = NULL;

    //
    this->r2 = r2;
    cal_directions();
    // printf("-- done: cal_directions\n     r2:%d\n     number of directions: %d\n", r2, dnum);

    //
    this->max_width = max_width;
    this->max_height = max_height;
    init_memory();
    create_lookup_table();
}

PolygonalIntegrator::~PolygonalIntegrator()
{
    if (vertical_sum != NULL)
    {
        for (int i=0; i<max_width; i++) delete [] vertical_sum[i];
        delete [] vertical_sum;
    }

    if (integral != NULL)
    {
        for (int i=0; i<dnum; i++)
        {
            for (int j=0; j<=max_width; j++) delete [] integral[i][j];
            delete [] integral[i];
        }
        delete [] integral;
    }

    if (lu_dir != NULL)
    {
        for (int i=0; i<=max_width*2; i++) delete [] lu_dir[i];
        delete [] lu_dir;
    }
}


bool PolygonalIntegrator::input_image(IplImage *img)
{
    this->img = img;

    //
    height = img->height;
    width  = img->width;
    wstep  = img->widthStep;
    img_data   = (uchar *) img->imageData;

    //printf("-- done: input image\n     image size: %dx%d\n", width, height);
    if (height>max_height || width>max_width)
    {
        printf("error: image size is %dx%d which is greater than max size %dx%d\n", width, height, max_width, max_height);
        return false;
    }

    //
    cal_vertical_sum();
    //printf("-- done: cal_vertical_sum\n");
    preprocess();
    //printf("-- done: preprocess\n");

    return true;
}

void PolygonalIntegrator::preprocess()
{
    // calculate 1
    for (int x=0; x<=width; x++) for (int y=0; y<=height; y++)
        integral[dnum1-1][x][y] = 0.0; // special case: dir = (0, -1)
    for (int d=0; d<dnum1-1; d++)  // exclude dir = (0, -1)
    {
        //
        for (int x=0; x<=width;  x++) integral[d][x][0] = 0.0;
        for (int y=0; y<=height; y++) integral[d][0][y] = 0.0;

        //
        cal_coefficient1(d);
        for (int y=1; y<=height; y++) for (int x=1; x<=width; x++)
        {
            integral[d][x][y] = cal_integral(d, x, y);
        }
    }

    // calculate 2
    for (int d=dnum1; d<dnum; d++)
    {
        //
        for (int x=0; x<=width;  x++) integral[d][x][0] = 0.0;
        for (int y=0; y<=height; y++) integral[d][width][y] = 0.0;

        //
        cal_coefficient2(d);
        for (int y=1; y<=height; y++) for (int x=0; x<width; x++)
        {
            integral[d][x][y] = cal_integral(d, x, y);
        }
    }
}

intype PolygonalIntegrator::integrate(const CvPolygon& p) // required: p.vertex[n] = p.vertex[0]
{
    int d;
    intype tmp, res = 0.0;

    for (unsigned i=0; i<p.size()-1; i++)
    {
        int x1 = p[i].x, y1 = p[i].y, x2 = p[i+1].x, y2 = p[i+1].y;
        if ((y1<y2) || (y1==y2 && x1<x2)) { swap(x1, x2); swap(y1, y2); }
        if(x1 == x2) continue;

        //
        d = lu_dir[(x2-x1)+max_width][-(y2-y1)];
        if (d==-1)
        {
            int g = binary_gcd(abs(x2-x1), abs(y2-y1));
            printf("error: does not support direction (%d, %d)\n", (x2-x1)/g, (y2-y1)/g);
            res = -1.0;
            break;
        }

        //
        tmp = integral[d][x1][y1] - integral[d][x2][y2];
        if (p[i].x <= p[i+1].x) res += tmp; else res -= tmp;
        //printf("debug: d=%d  xy1=%d %d  xy2=%d %d  tmp=%.2lf  integral 1: %.2lf  2: %2.lf\n", d, x1, y1, x2, y2, tmp, integral[d][x1][y1], integral[d][x2][y2]);
    }

    return res;
}

//------------------------------------------------------------------------------
void PolygonalIntegrator::init_memory()
{
    //
    vertical_sum = new int*[max_width];
    for (int i=0; i<max_width; i++) vertical_sum[i] = new int[max_height+1];

    //
    integral = new intype**[dnum];
    for (int i=0; i<dnum; i++)
    {
        integral[i] = new intype*[max_width+1];
        for (int j=0; j<=max_width; j++) integral[i][j] = new intype[max_height+1];
    }

    //
    lu_dir = new int*[max_width*2+1];
    for (int i=0; i<=max_width*2; i++) lu_dir[i] = new int[max_height+1];
}

void PolygonalIntegrator::cal_directions()
{
    int x, y, r = int(sqrt(r2)) + 1;

    //
    dnum1 = 0;
    for (x=-r; x<=0; x++) for (y=-r; y<=0; y++) if (x*x+y*y<=r2 && binary_gcd(abs(x), abs(y))==1)
    {
        dx[dnum1] = x, dy[dnum1++] = y;
    }

    //
    dnum = dnum1;
    for (x=1; x<=r; x++) for (y=-r; y<0; y++) if (x*x+y*y<=r2 && binary_gcd(abs(x), abs(y))==1)
        dx[dnum] = x, dy[dnum++] = y;
}


void PolygonalIntegrator::create_lookup_table()
{
    int x, y, g;

    //
    for (x=-max_width; x<=max_width; x++) for (y=-max_height; y<=0; y++) lu_dir[x+max_width][-y] = -1;
    for (int i=0; i<dnum; i++) lu_dir[dx[i]+max_width][-dy[i]] = i;

    //
    for (x=-max_width; x<=max_width; x++) for (y=-max_height; y<=0; y++) if (!(x==0 && y==0))
    {
        g = binary_gcd(abs(x), abs(y));
        if (g==0) continue;
        lu_dir[x+max_width][-y] = lu_dir[x/g+max_width][-y/g];
    }
}

void PolygonalIntegrator::cal_vertical_sum()
{
    for (int x=0; x<width; x++)
    {
        vertical_sum[x][0] = 0;
        for (int y=1; y<=height; y++) vertical_sum[x][y] = vertical_sum[x][y-1] + img_data[(y-1)*wstep + x];
    }
}

void PolygonalIntegrator::add_coe(intype v, int x, int y)
{
    coe_v[coe_n] = v; coe_x[coe_n] = x; coe_y[coe_n] = y;
    coe_n++;
}

void PolygonalIntegrator::cal_coefficient1(int d)  // dx[d] != 0
{
    int u = dx[d], v = dy[d];  // assume x=0, y=0
    intype le, ri, big, little, stepy = fabs((intype) dy[d]/dx[d]);
    int ile, iri;

    ver_x = u; ri = v; coe_n = 0;
    for (int i=u; i<0; i++)
    {
        le = ri; ri += stepy;
        ile = (int) floor(le); iri = (int) floor(ri);   // eps is necessary?
        ver_y[i-u] = ile;

        //
        if (ile == iri) { add_coe(0.5*(le-ile+ri-iri), i, iri); continue; }

        //
        big = (i+1-u) - (ile+1-v)/stepy;
        add_coe(1 - 0.5*(ile+1-le)*(1-big), i, ile);
        for (int j=ile+2; j<=iri; j++)
        {
            little = (i+1-u) - (j-v)/stepy;
            add_coe(0.5*(little+big), i, j-1);
            big = little;
        }
        add_coe(0.5*(ri-iri)*big, i, iri);
    }
}

void PolygonalIntegrator::cal_coefficient2(int d)  // dx[d] != 0, note: le and ri are reversed
{
    int u = dx[d], v = dy[d];  // assume x=0, y=0
    intype le, ri, big, little, stepy = fabs((intype) dy[d]/dx[d]);
    int ile, iri;

    ver_x = 0; ri = v; coe_n = 0;
    for (int i=u-1; i>=0; i--)
    {
        le = ri; ri += stepy;
        ile = (int) floor(le); iri = (int) floor(ri);   // eps is necessary?
        ver_y[i] = ile;

        //
        if (ile == iri) { add_coe(0.5*(le-ile+ri-iri), i, iri); continue; }

        //
        big = (u-i) - (ile+1-v)/stepy;
        add_coe(1 - 0.5*(ile+1-le)*(1-big), i, ile);
        for (int j=ile+2; j<=iri; j++)
        {
            little = (u-i) - (j-v)/stepy;
            add_coe(0.5*(little+big), i, j-1);
            big = little;
        }
        add_coe(0.5*(ri-iri)*big, i, iri);
    }
}


intype PolygonalIntegrator::cal_integral(int d, int x, int y)  // dx[d] != 0
{
    int u=x+dx[d], v=y+dy[d];
    intype res = 0.0;


    if (u>=0 && v>=0 && u<=width && v<=height) res += integral[d][u][v];
    for (int i=0; i<coe_n; i++)
    {
        u = x+coe_x[i]; v = y+coe_y[i];
        if (u>=0 && v>=0 && u<width && v<height) res += coe_v[i] * img_data[v*wstep + u];
    }
    //if (d==58 && x==319 && y==1) printf("debug: res=%2lf      %2lf\n", res, res-integral[d][u][v]);

    for (int i=0; i<abs(dx[d]); i++)
    {
        u = x+ver_x+i; v = y+ver_y[i];
        if (u>=0 && v>=0 && u<width && v<=height) res += vertical_sum[u][v];
    }
    return res;
}
